/************************************************

     Brian Bousquet-Smith     Chaz Godwin
                
            OBDII Reader
                 Fall 2011


NOTES:

Code designed for use on an ATMEGA164A AVR. If this
is not the chip being used unknown errors may be 
involved in the running of this code. 
************************************************/

#include <avr/io.h>
#include <avr/iom164.h> 
#include <avr/interrupt.h>
#include <util/delay.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>


/************************************************
             Defines
************************************************/

// States

//#define F_CPU            1000000UL
#define ELM_INIT_STATE            1
#define    OBD_INIT_STATE            2
#define BLUETOOTH_INIT_STATE        3
#define DASHBOARD            4

// Error Defines

#define OK                     0
#define OVERFLOW                -1    
#define BAD_DATA                -2
#define NO_DEVICE_FOUND                -3

/*
*    Error Meanings
*
*    Overflow - Buffer limit of 100 was reached before receiving an end of command byte 
*    Bad Data - Response from the OBDII wasn't the data expected as the reponse
*    No Device Found - Output when the ELM can't find an active protocol to communicate with
*    Impossible Data - Result from the OBDII lies outside the bounds of the data
*    Unknown - This response isn't at all expected
*
*/

// OBD Defines

#define COMMAND_END    0x0D
#define RESPONSE_END    0x3E


/************************************************
        Function Prototypes
************************************************/

// Initializing Functions

void power_on();
void serial_init();
int elm_init();
int obd_init();
int bluetooth_init();
int get_mph();
int get_rpm();

// OBD Functions

void obd_transmit(unsigned char data);
unsigned char obd_receive();
void obd_flush();
int obd_sendcmd(unsigned char *in, unsigned char *out, char len_in);
int check_data(unsigned char *resp, unsigned char *exp, int len_exp);
int record_data(unsigned char *out, unsigned char *rec, int offset, int len_in);

// LCD Functions


int print_lcd(const char *error);
void lcd_transmit(unsigned char data);
unsigned char lcd_receive();
void lcd_flush();
int lcd_sendcmd(unsigned char *in, unsigned char *out, char len_in);
void clear_lcd(void);


// Debugging Functions

void print_bits(char byte);
void newline();

/************************************************
        Global Variables
************************************************/

int main_state = BLUETOOTH_INIT_STATE;
int timeoutflag = 0;

/************************************************
        Main Code
************************************************/

int main(void){

/*
*    Purpose: Output RPM and MPH data from a vehicle
*/    
    DDRA = 0xFF;
    serial_init();
    lcd_flush();
    int mph;
    int rpm;
    int response = -1;    

/************************************************
        State Machine
************************************************/
    //_delay_ms(5000);
    //clear_lcd();
    while(1){
        switch(main_state){

        // Initialization States    

        case ELM_INIT_STATE: // Initializes ELM
            response = elm_init();
            if(response == OK){    
                main_state = OBD_INIT_STATE;
                break;
            }
            print_error(response);
            break;

        case OBD_INIT_STATE: // Initializes OBDII
            obd_flush();
            response = obd_init();
            if(response == OK){
                main_state = DASHBOARD;                
                break;
            }
            print_error(response);
            break;

        case BLUETOOTH_INIT_STATE: // Initializes Bluetooth        
            PORTA = 0x00;
            _delay_ms(1000);
            response = bluetooth_init();
            PORTA = 0x03;    
            if(response == 0){
                main_state = DASHBOARD;
                break;
            }
            print_error(response);
            break;    

        case DASHBOARD: // Virtual Dashboard        
                
            //mph = get_mph();
            //rpm = get_rpm();
            _delay_ms(200);
            lcd_transmit('1');
            lcd_transmit('2');
            lcd_transmit('3');
            lcd_transmit('4');
            lcd_transmit('5');
            lcd_transmit('6');
            break;
            //}
            //else{
            //    main_state = ELM_INIT_STATE;
            //    print_error(response);
            //    break;
            //}
            //c har rpm[2]; 
            //response = get_rpm(rpm);
            //if(response == 0){
            //    lcd_transmit(0x00);
            //    lcd_transmit(0x00);
            //}
            //else{
            //    main_state = ELM_INIT_STATE;
            //    print_error(response);
            //    break;
            //}            
        } 
    }
    return 0;
}

void serial_init(){

/*
*    Purpose: Initialize Serial on Both UART Ports on the AVR
*    Input: Nothing
*    Output: Nothing
*/

// ELM

    UCSR0A |= (1<<U2X0);                // Double speed for more precision
    UCSR0B |= (1<<RXEN0)|(1<<TXEN0);    // Allow transmit and receive
    UCSR0C |= (0<<USBS0)|(3<<UCSZ00);     // 8 Bit No Parity 1 Stop Bit
    UBRR0H = 0;                            // Sets Baud Rate = 9600
    UBRR0L = 12;

// LCD

    UCSR1A |= (1<<U2X1);                // Double speed for more precision
    UCSR1B |= (1<<RXEN1)|(1<<TXEN1);     // Allow transmit and receive
    UCSR1C |= (0<<USBS1)|(3<<UCSZ10);     // 8 Bit No Parity 1 Stop Bits
    UBRR1H = 0;                            // Sets Baud Rate = 9600
    UBRR1L = 12;
}

int elm_init(){

/*
*    Purpose: Initialize ELM and check that communication is correct
*    Input: Nothing
*    Output:    Returns OK or corresponding error codes
*/
    int error;
    unsigned char in_buff[3] = {'A', 'T', 'Z'};
    unsigned char exp_buff[15] = {'A', 'T', 'Z', 'E', 'L', 'M', '3', '2', '7', 'v', '1', '.', '4', 'b', 0x3E};
    unsigned char out_buff[100];        
    

    // Get the response from the ELM 
    error = obd_sendcmd(in_buff, out_buff, sizeof(in_buff));    
    if(error == OVERFLOW){
        return error;
    }
    // Check that data is as expected

    error = check_data(out_buff, exp_buff, sizeof(exp_buff));
    if(error == BAD_DATA){
        return error;
    }
    return OK;
    
}
    
int bluetooth_init(){

/*
*    Purpose: Initialize the Bluetooth Device and check that communication is correct
*    Input: Nothing
*    Output: Returns OK or corresponding error codes
*/


    //Based on the RTADDR it can be determined if the baud rate and everything is correct
    //Also if it responds to this then we know then the state is 1
    //_delay_ms(2000);
    //lcd_flush();    
    unsigned char in_buff[5] = "\r\n2\r\n";
    unsigned char out_buff[100];
    unsigned char error[5] = "ERROR";    
    int result;
    int i;
    // Check if connection already made
    lcd_transmit('\r');
    lcd_transmit('\n');
    lcd_transmit('1');
    lcd_transmit('\r');
    lcd_transmit('\n');
    char tmp;
    PORTA = 0x01;
    i = 0;
    /*while(tmp != 'E'){
        tmp = lcd_receive();
        PORTA = 0x03;
        out_buff[i] = tmp;
        i++;
    } */
    lcd_receive();
    lcd_receive();
    lcd_receive();
    lcd_receive();
    lcd_receive();
    
    while((tmp = lcd_receive()) != '\r');
    lcd_receive();
    while(1){
        out_buff[i] = lcd_receive();
        if(i==100){
            return OVERFLOW;
        }
        if(out_buff[i] == '\r') break;
        i++;
    }
    int j = 0;
    /*for(j=0;j<i;j++){
        PORTA = 0x00;
        _delay_ms(250);
        PORTA = 0x03;
        _delay_ms(250);
    }
    while(1);*/
        //lcd_sendcmd(in_buff, out_buff, sizeof(in_buff));    
    result = strncmp(error, out_buff, sizeof(error));
    if(result == 0){    
        result = lcd_listen();
        return result;    
    }
    result = strncmp(in_buff, out_buff, sizeof(in_buff));
    if(result == 0){
        return 0;
    }
    else{        
        return BAD_DATA;
    }
}

int lcd_listen(){
/*
*    Purpose: Device listens and waits for a response from the Bluetooth Bee
*    Input: Nothing
*    Output:
*/

    char state4[10] = "+BTSTATE:4";    
    char response[100];
    char response2[5];
    int result = 0;    
    int not_connected = 1;
    unsigned char tmp;
    PORTA = 0x01;
    int i;
    while(not_connected){
        PORTA = 0x01;
        while(lcd_receive() != '\r');
        while(lcd_receive() != '\n');
        for(i=0;i<100;i++){
            tmp = lcd_receive();
            if(tmp != '\r'){
                response[i] = tmp;
            }
            else{
                break;
            }
        }
        result = strncmp(state4, response, sizeof(state4));    
        _delay_ms(20);
        if(result == 0){
            //print_error(NO_DEVICE_FOUND);
            PORTA =0x02;
            lcd_flush();
            //_delay_ms(1000);
            char rep[5] = "\r\n2\r\n";
            lcd_transmit('\r');
            lcd_transmit('\n');
            lcd_transmit('1');
            lcd_transmit('\r');
            lcd_transmit('\n');    
            for(i=0;i<sizeof(response2);i++) response2[i] = lcd_receive();    
            result = strncmp(rep, response2, sizeof(rep));
            result = 0;            
            if(result == 0){
                PORTA = 0x00;
                _delay_ms(1000);
                lcd_transmit('\r');
                lcd_transmit('\n');
                lcd_transmit('1');
                lcd_transmit('\r');
                lcd_transmit('\n');
                return OK;
            }
            else{
                PORTA = 0x03;
                return BAD_DATA;
            }            
        }    
    }    
    return 0;    
}

int obd_init(){

/*
*    Purpose: Initialize OBDII port and check that communication is correct
*    Input: Nothing
*    Output: Returns OK if successful otherwises returns corresponding error codes
*/
    
    unsigned char in_buff[4] = {'0', '1', '0', '0'};
    unsigned char exp_buff1[31] = "0100SEARCHING...UNABLETOCONNECT";
    unsigned char exp_buff2[20] = "0100SEARCHING...4100";
    unsigned char exp_buff3[8] = "01004100";
    unsigned char out_buff[100];
    int error = 0;
    int i;
    
    
    // Send command to ELM

    error = obd_sendcmd(in_buff, out_buff, sizeof(in_buff));
    if(error == OVERFLOW){
        return error;
    }

    // Find protocol    

    error = check_data(out_buff, exp_buff1, sizeof(exp_buff1));
    if(error == BAD_DATA){    // Means something other than unable to connect was seen
        error = check_data(out_buff, exp_buff2, sizeof(exp_buff2));
        if (error == BAD_DATA){    // Means didn't match correctly with expected buff 2
            error = check_data(out_buff, exp_buff3, sizeof(exp_buff3));
            if(error == BAD_DATA){ // Didn't match any of the expected buffer, must be erro
                return BAD_DATA;
            }
            return OK;    // Matched 3rd expected buffer so its ready to get data
        }
        return OK; // Matched 2nd expected buffer so its ready to get data
    }
    return NO_DEVICE_FOUND;    
}

int get_mph(void){

/*
*    Purpose: Get MPH Data and Output it
*    Input: Nothing
*    Output: Mph of the vehicle or an error code, refer to error codes for their meaning
*/
    static int MPH_MAX = 128;
    static int MPH_MIN = 0;
    int i = 0, error = 0, count;
    unsigned int mph;
    unsigned char in_buff[4] = {'0', '1', '0', 'D'}; // Input Command Buffer
    unsigned char exp_buff[8] = {'0', '1', '0', 'D', '4', '1', '0', 'D'}; // Expected Data Buffer
    unsigned char out_buff[100];    // Buffer to fill with data 
    unsigned char mph_buff[3];     // Buffer to fill with actual MPH

    // Send command and get data back from ELM/OBDII

    error = obd_sendcmd(in_buff, out_buff, 4); // Send command
    if(error == OVERFLOW){
        return error;
    }
    count = check_data(out_buff, exp_buff, sizeof(exp_buff));
    if(count < 0){
        return BAD_DATA;
    }
    // Record MPH Data
    error = record_data(out_buff, mph_buff, count, sizeof(mph_buff)-1);
    if(error == OVERFLOW){
        return error;
    }
    // Convert to MPH

    mph =(unsigned int)strtol((char *)mph_buff, NULL, 16);
    mph = mph*.621371;    // Converts KMPH to MPH    
    
    // Variable size based on how big mph is
    if(mph>MPH_MAX||mph<MPH_MIN){    // MPH is less than 0 or greater than 128...both impossible
        return -4;
    }
    return mph;
}


int get_rpm(void){

/*
*    Purpose: Get RPM Data and Output it
*    Input: Nothing
*    Output: Outputs the rpm of the car or an error code, refer to error codes for their meaning
*/
    static int RPM_MAX = 16383;
    static int RPM_MIN = 0;
    int i=0, j=0, error = 0, count = 0;
    unsigned int rpm;
    unsigned char error_buff[15];
    unsigned char in_buff[4] = {'0', '1', '0', 'C'};    
    unsigned char exp_buff[8] = {'0', '1', '0', 'C', '4', '1', '0', 'C'};     
    unsigned char out_buff[100];
    unsigned char rpm_buff[5]; 
    error = obd_sendcmd(in_buff, out_buff, 4);
    if(error == OVERFLOW){
        return error;
    }
    count = check_data(out_buff, exp_buff, sizeof(exp_buff));
    if(count<0){
        return BAD_DATA;
    }
    error = record_data(out_buff, rpm_buff, count, sizeof(rpm_buff)-1);
    if(error == OVERFLOW){
        return error;
    }

    rpm =(unsigned int)strtol((char *)rpm_buff, NULL, 16);    
    rpm = rpm/4;

    if(rpm>RPM_MAX||rpm<RPM_MIN){    // MPH is less than 0 or greater than 128...both impossible
        return -4;
    }

    return rpm;
}


int check_data(unsigned char *resp, unsigned char *exp, int len_exp){

/*
*    Purpose: Check to make sure the data is as expected
*    Input:    resp     - Response from the ELM/OBDII
*        exp    - Expected response
*        len_exp    - Length of expected response
*    Output: >0     - Location in the output buffer of the data
*        -1    - Buffer didn't contain correct data
*/
    int i;
    int j = 0;
    for(i=0;i<len_exp;i++){    
        obd_transmit(resp[j]);
        while( (resp[j]=='\n') || (resp[j]=='\r') || (resp[j]==' ')) j++; // Ignore all new lines/carriage returns/spaces
        obd_transmit(resp[j]);        
        if(resp[j]!=exp[i]){
            // Check to make sure data is what is expected
            return BAD_DATA;    // If not return
        }
        j++;    
    }    
    return j;
}


int lcd_sendcmd(unsigned char *in, unsigned char *out, char len_in){

/*
*    Purpose: Send a string to the LCD to print
*    Input:    in - Input buffer to be printed
*        len-in - Length of the buffer to be printed
*    Output: Error Codes:
*        0 - Successful
 *        -1 - Unsuccessful
*/    

    int i;
    for(i=0;i<len_in;i++){
        lcd_transmit(in[i]);        
    }
    // Output Response in Buffer
    lcd_receive();
    lcd_receive();
    for(i=0;i<100;i++){
        out[i] = lcd_receive();
        if (out[i] == '\r'){
            return OK; // When end is reached, return
        }
        
    }
    return OVERFLOW;      
}

int obd_sendcmd(unsigned char *in, unsigned char *out, char len_in){

/*
*    Purpose: Send a command to ELM to run
*    Input:      in     - Command buffer        
*         out     - Location to output results
*         len_in - Length of the input
*    Output: Error Codes
*          OK         - Successful
*        Overflow    - Longer than 100 bytes before end of data
*/        

    //TODO Start Interrupt
    int i;
    obd_flush();            // Kill all data in the obd buffer
    
    // Transmit Command to the ELM

    for(i=0;i<len_in;i++){        
        obd_transmit(in[i]);    
    }
    obd_transmit(COMMAND_END); //End byte for the command
    // Output Response in Buffer
    for(i=0;i<100;i++){
        out[i] = obd_receive();
        if (out[i] == RESPONSE_END){
            return OK; // When end is reached, return
        }
    }
    return OVERFLOW; 
}

/*void int_timer(void){
    TCCR1B |= (1<<WGM12);
    TIMSK |= (1 << OCIE1A);
    sei();
    OCR1A = 31249;
    TCR1B |= (1<<CS11)|(1<<CS10);
}

ISR(*/

void lcd_transmit (unsigned char data){
    
/*
*    Purpose: Transmit data to the LCD
*    Input: Data to be transmitted
*    Output:    Nothing
*/

    while( !(UCSR1A & (1<<UDRE1))); 
    UDR1 = data;
}

unsigned char lcd_receive (void){

/*
*    Purpose: Receive data from the LCD
*    Input: Nothing
*    Output:    Data received    
*/

    while (!(UCSR1A & (1<<RXC1)));
    return UDR1;
}    

void lcd_flush(void){

/*
*    Purpose: Clear all old data in the buffer
*    Input: Nothing
*    Output: Nothing
*/

    char tmp;
    while (UCSR1A & (1<<RXC1) ) tmp = UDR1;
}

void obd_transmit (unsigned char data){

/*
*    Purpose: Transmit data to the LCD
*    Input: Data to be transmitted
*    Output:    Nothing
*/

    while( !(UCSR0A & (1<<UDRE0)));
    UDR0 = data;
}

unsigned char obd_receive (void){

/*
*    Purpose: Receive data from the LCD
*    Input: Nothing
*    Output:    Data received    
*/
    while (!(UCSR0A & (1<<RXC0)));
    return UDR0;
}

void obd_flush(void) {

/*
*    Purpose: Receive data from the LCD
*    Input: Nothing
*    Output:    Nothing    
*/

    char tmp;
    while( UCSR0A & (1<<RXC1)) tmp = UDR0;
}

int print_lcd(const char *error){

/*
*    Purpose: To print an error to the llllluser
*    Input: String to be printed ****MUST BE ENDED WITH A NEWLINE!!!****
*
*/    
    unsigned char tmp[100];
    int i = 0;
    while(error[i] != '\n'){
        if(i>=100) return OVERFLOW; // TODO Better way?
        tmp[i] = error[i];
        i++;
    }
    int j;
    unsigned char tmp2[i];
    unsigned char error_buff[i];
    for(j=0;j<i;j++){
        tmp2[j] = tmp[j];
    }
    sprintf((char *)error_buff, (char *)tmp2); // If not send an error
    //lcd_sendcmd(error_buff, sizeof(error_buff));
    return 0;
}    

int get_size(int data){
    int bound1 = 0;
    int bound2 = 10;
    int dig = 0;
    while(data >= bound1 && data < bound2){
        bound1 = bound2;
        bound2 *= 10;
        dig++;
    }
    return dig;
}

void clear_lcd(void){
    lcd_transmit(0x7C);
    lcd_transmit(0x00);
}

int record_data(unsigned char *out, unsigned char *rec, int offset, int len_in){
    int i;
    for(i=0;i<len_in;i++){
        while( (out[offset]=='\n') || (out[offset]=='\r') || (out[offset]==' ')) offset++;
        if(offset >= 100) return OVERFLOW;
        rec[i] = out[offset];
        offset++;
    }
    rec[i] = '\0'; 
    return OK;
}    

void print_error(int error){
    PORTA = 0x03;
    _delay_ms(1000);
    char times;    
    switch(error){
        case OVERFLOW:
            times = 1;
            break;
        case BAD_DATA:
            times = 2;
            break;
        case NO_DEVICE_FOUND:
            times = 3;
            break;
    }
    int i;
    for(i=0;i<times;i++){
        PORTA = 0x00;
        _delay_ms(250);
        PORTA = 0x03;
        _delay_ms(250);
    }
}

void print_bits( char byte){

/*
*    DEBUGGING FUNCTION
*    Purpose: Print the bits of an ascii byte so that its easier to figure out what it is 
*    Input: Byte that needs to be written in bits
*    Output: Nothing
*/

    char mask[8] = {
    0b10000000,
    0b01000000,
    0b00100000,
    0b00010000,
    0b00001000,
    0b00000100,
    0b00000010,
    0b00000001};

    int m;
    char tmp;
    for(m=0;m<8;m++){
        tmp = ((mask[m]&byte)>>(7-m))+48;
        obd_transmit(tmp);
    }
    obd_transmit('\r');
    obd_transmit('\n');
}        
